﻿#pragma once
#include "AsAlgorithmGlobal.h"
#include <opencv2\core.hpp>
#include <AsDebug.h>
#define OPENCV_DEP
#define AS_MATH_API AS_ALGORITHM_EXPORT
#define ASMATH_CHECK_POINT_SIZE(VEC, NUM, RET) if (VEC.size() < NUM) { AsDebug() << "[AsMath] points too low:" << VEC.size(); RET;}

namespace AsMathContext {

	extern cv::Point2f AS_MATH_API offset;

}

namespace AsMath {

	const double PI = 3.141592654;
	const double PI_D2 = PI / 2;
	
	const int INVALID_INDEX = -1;

	cv::Vec3f AS_MATH_API UnitVecX();
	cv::Vec3f AS_MATH_API UnitVecY();
	cv::Vec3f AS_MATH_API UnitVecZ();

	bool AS_MATH_API isZero(float f);
	bool AS_MATH_API isZero(double f);

	bool AS_MATH_API isEqual(float f1, float f2);
	bool AS_MATH_API isEqual(double f1, double f2);

	double AS_MATH_API angle2Radian(double angle);
	double AS_MATH_API radian2Angle(double radian);
	
	cv::Point AS_MATH_API midPoint(cv::Point p1, cv::Point p2);

	cv::Rect AS_MATH_API circleRect(const cv::Point& center, const int radius);

	double AS_MATH_API calcDistance(const cv::Point& p1, const cv::Point& p2);
	double AS_MATH_API calcDistance(const cv::Point2f& p1, const cv::Point2f& p2);
	double AS_MATH_API calcDistance(const cv::Point2d& p1, const cv::Point2d& p2);
	
	cv::Point2f AS_MATH_API calcDistanceComponent(double distance, double slope);

	double AS_MATH_API calcRadian(cv::Vec3d vec1, cv::Vec3d vec2);

	cv::Mat AS_MATH_API calcTranslateMat(cv::Vec3d distance);

	cv::Mat AS_MATH_API calcRotationMat(cv::Vec3d srcVector, cv::Vec3d dstVector);

	std::vector<cv::Point2d> AS_MATH_API calcIntersect_circleLine(double x0, double y0, double r, double a, double b, double c);

	cv::Mat AS_MATH_API vector2Mat(const std::vector<cv::Point3f>& inputPoints, bool isHorizontal = false);
	cv::Mat AS_MATH_API vector2Mat(const std::vector<cv::Point3f>& inputPoints, bool isHorizontal, float supplementary);
	void AS_MATH_API mat2Vector(const cv::Mat& inputMat, std::vector<cv::Point3f>& outputVector, bool isHorizontal = false);

};

namespace AsMath {

	bool AS_MATH_API hasPoint(const std::vector<cv::Point>& points, cv::Point point);
	void AS_MATH_API minmax(const std::vector<cv::Point>& points, int& minx, int& miny, int& maxx, int& maxy);

	cv::Rect AS_MATH_API maxRect(const std::vector<cv::Point>& points);
	void AS_MATH_API getSame(const std::vector<cv::Point>& points1, const std::vector<cv::Point>& points2, std::vector<cv::Point>& outputPoints);
	
	void AS_MATH_API interpolation(const cv::Point& p1, const cv::Point& p2, std::vector<cv::Point>& outputPoints, int interval);
	void AS_MATH_API interpolation(const std::vector<cv::Point>& inputPoints, std::vector<cv::Point>& outputPoints);
	void AS_MATH_API interpolation(const std::vector<cv::Point>& inputPoints, std::vector<cv::Point>& outputPoints, double minDistance, double maxDistance);

};

namespace AsMath {

	int AS_MATH_API indexNearest(const std::vector<int>& values, const int compareValue);
	int AS_MATH_API indexMaximun(const std::vector<int>& values);

	std::vector<int> AS_MATH_API indexsNearest(const std::vector<double>& inputArr, double value);
	std::vector<int> AS_MATH_API indexsSeqDiachotomies(size_t size);
	std::vector<int> AS_MATH_API indexsExtremumMax(const std::vector<int>& values, const int stableStep = 1);
	std::vector<int> AS_MATH_API indexsExtremumMin(const std::vector<int>& values, const int stableStep = 1);

};

namespace AsMath {

	std::vector<int> AS_MATH_API around(const std::vector<int> inputValues, int inputValue);

};

namespace AsMath {

	cv::Point AS_MATH_API minX(const std::vector<cv::Point>& inputPoints);
	cv::Point AS_MATH_API minY(const std::vector<cv::Point>& inputPoints);
	cv::Point AS_MATH_API maxX(const std::vector<cv::Point>& inputPoints);
	cv::Point AS_MATH_API maxY(const std::vector<cv::Point>& inputPoints);

	float AS_MATH_API minimumX(const std::vector<cv::Point3f>& input);
	float AS_MATH_API minimumY(const std::vector<cv::Point3f>& input);
	float AS_MATH_API minimumZ(const std::vector<cv::Point3f>& input);
	float AS_MATH_API maximumX(const std::vector<cv::Point3f>& input);
	float AS_MATH_API maximumY(const std::vector<cv::Point3f>& input);
	float AS_MATH_API maximumZ(const std::vector<cv::Point3f>& input);
	float AS_MATH_API meanX(const std::vector<cv::Point3f>& input);
	float AS_MATH_API meanY(const std::vector<cv::Point3f>& input);
	float AS_MATH_API meanZ(const std::vector<cv::Point3f>& input);

};
/*
	Debug Methods
*/
namespace AsMath {
	
	template <typename _Type>
	void printVector(const std::vector<_Type>& inputVector, int printSize = -1) {
		AsDebug(0);
		if (inputVector.empty()) {
			std::cout << "[AsMath] Print Vector: InputVector Is Empty" << std::endl;
		}
		else std::cout << "[AsMath] Print Vector:" << std::endl;
		if (printSize < 0) {
			AsDebug(0);
			for (const auto& elem : inputVector) {
				std::cout << elem << " ";
			}
			std::cout << std::endl;
		}
		else {
			AsDebug(0);
			for (size_t i = 0; i < inputVector.size() && i < printSize; i++) {
				std::cout << inputVector[i] << " ";
			}
			std::cout << std::endl;
		}
	}

};
/*
	General Vector Methods
*/
namespace AsMath {

	template <typename _Type1, typename _Type2>
	std::vector<_Type2> vector_cast(std::vector<_Type1>& vec) {
		std::vector<_Type2> output;
		output.reserve(vec.size());
		for (const auto& e : vec) {
			output.emplace_back(static_cast<_Type2>(e));
		}
		return std::move(output);
	}

	template <typename _Type>
	typename std::vector<_Type>::const_iterator it(const std::vector<_Type>& vec, const _Type& elem) {
		auto cbegin = vec.cbegin();
		auto cend = vec.cend();
		typename std::vector<_Type>::const_iterator it;
		for (it = cbegin; it != cend; ++it) {
			if (*it == elem) break;
		}
		return it;
	}

	template <typename _Type>
	bool contains(const std::vector<_Type>& vec, const _Type& elem) {
		for (const auto& e : vec) {
			if (e == elem) return true;
		}
		return false;
	}

	template <typename _Type>
	bool remove(std::vector<_Type>& vec, const _Type& elem) {
		auto it = AsMath::it(vec, elem);
		if (it != vec.end()) {
			vec.erase(it);
		}
	}

	template <typename _Type>
	void removeRepeat(std::vector<_Type>& vec) {
		std::vector<_Type> tmp;
		const _Type* lst = nullptr;
		tmp.reserve(vec.size());
		for (const auto& e : vec) {
			if (lst == nullptr) {
				tmp.emplace_back(e);
			}
			else {
				if (e != *lst) {
					tmp.emplace_back(e);
				}
			}
			lst = &e;
		}
		vec = std::move(tmp);
	}

	template <typename _Type, typename _Functor>
	void traverse(std::vector<_Type>& vec, _Functor functor) {
		auto begin = vec.begin();
		auto end = vec.end();
		for (auto it = begin; it != end; ++it) {
			if (!functor(*it)) return;
		}
	}

	template <typename _Type, typename _Functor>
	void traverse_i(std::vector<_Type>& vec, _Functor functor) {
		auto begin = vec.begin();
		auto end = vec.end();
		int index = 0;
		for (auto it = begin; it != end; ++it, ++index) {
			if (!functor(*it, index)) return;
		}
	}

	template <typename _Type, typename _Functor>
	void traverse_const(const std::vector<_Type>& vec, _Functor functor) {
		auto begin = vec.cbegin();
		auto end = vec.cend();
		for (auto it = begin; it != end; ++it) {
			if (!functor(*it)) return;
		}
	}

	template <typename _Type, typename _Functor>
	void traverse_i_const(const std::vector<_Type>& vec, _Functor functor) {
		auto begin = vec.cbegin();
		auto end = vec.cend();
		int index = 0;
		for (auto it = begin; it != end; ++it, ++index) {
			if (!functor(*it, index)) return;
		}
	}
};

namespace AsMath {

	template <typename _Type>
	cv::Point_<_Type> getNearest(const std::vector<cv::Point_<_Type>>& points, const cv::Point_<_Type>& target) {
		ASMATH_CHECK_POINT_SIZE(points, 1, return cv::Point_<_Type>());
		int nearestIndex = 0;
		int minDist = INT_MAX;
		for (int i = 0; i < points.size(); i++) {
			double dist = AsMath::calcDistance(points[i], target);
			if (minDist > dist) {
				minDist = dist;
				nearestIndex = i;
			}
		}
		return points[nearestIndex];
	}

	template <typename _Type>
	bool sameSign(_Type n1, _Type n2) {
		return n1 > 0.0 && n2 > 0.0 || n1 < 0.0 && n2 < 0.0;
	}

	template <typename _Type>
	void swap(_Type& n1, _Type& n2) {
		_Type tmp = n1;
		n1 = n2;
		n2 = tmp;
	}

	template <typename _Type>
	void swap(std::vector<_Type>& array, size_t index1, size_t index2) {
		_Type tmp = array[index1];
		array[index1] = array[index2];
		array[index2] = tmp;
	}

	template <typename _Type>
	_Type mid(std::vector<_Type>& array) {
		return array[array.size() / 2];
	}

	template <typename _Type>
	_Type rand(std::vector<_Type>& array) {
		return array[std::rand() % array.size()];
	}

	template <typename _Type>
	void append(std::vector<_Type>& vec1, const std::vector<_Type>& vec2) {
		for (const auto& e : vec2) {
			vec1.emplace_back(e);
		}
	}

	template <typename _Type>
	_Type minimum(_Type a, _Type b) {
		return a < b ? a : b;
	}

	template <typename _Type>
	_Type minimum(const std::vector<_Type>& input) {
		_Type ret = input.front();
		for (auto i = input.begin(); i != input.end(); i++) {
			if (ret > *i) ret = *i;
		}
		return ret;
	}

	template <typename _Type>
	_Type maximum(_Type a, _Type b) {
		return a > b ? a : b;
	}

	template <typename _Type>
	_Type maximum(const std::vector<_Type>& input) {
		_Type ret = input.front();
		for (auto i = input.begin(); i != input.end(); i++) {
			if (ret < *i) ret = *i;
		}
		return ret;
	}

	template<typename _Type>
	_Type maximum(const std::vector<_Type>& input, size_t& index) {
		_Type ret = input.front();
		size_t _index = 0;
		for (const auto& value : input) {
			if (ret < value) {
				ret = value;
				index = _index;
			}
			_index++;
		}
		return ret;
	}

	template <typename _ContainerType, typename _DataType>
	_DataType minimumX(const _ContainerType& input) {
		_DataType temp = input.front();
		for (const auto& elem : input) {
			if (temp > elem) temp = elem;
		}
		return temp;
	}

	template<typename _Type>
	_Type accumulate(const std::vector<_Type>& array) {
		_Type ret = 0;
		for (const auto& e : array) {
			ret += e;
		}
		return ret;
	}

	template<typename _Type>
	_Type mean(const std::vector<_Type>& array) {
		if (array.empty()) return 0;
		_Type ret = accumulate(array) / (_Type)array.size();
		return ret;
	}

	template<typename _Type>
	_Type median(const std::vector<_Type>& array) {
		if (array.empty()) return 0;
		std::vector<_Type> tmpArr = array;
		std::sort(tmpArr.begin(), tmpArr.end());
		return tmpArr[tmpArr.size() / 2];
	}

	template<typename _Type>
	_Type squdev(const std::vector<_Type>& array) {
		if (array.size() < 2) return 0;
		_Type mean_val = mean(array);
		_Type ret = 0;
		for (const auto& e : array) {
			ret += pow(e - mean_val, 2);
		}
		return ret / array.size();
	}

	template<typename _Type>
	_Type stddev(const std::vector<_Type>& array) {
		if (array.size() < 2) return 0;
		_Type mean_val = mean(array);
		_Type ret = 0;
		for (auto i = array.begin(); i != array.end(); i++) {
			ret += (*i - mean_val) * (*i - mean_val);
		}
		return sqrt(ret / (array.size() - 1));
	}

	template<typename _Type>
	_Type stddev(const std::vector<_Type>& array, _Type mean_val) {
		if (array.size() < 2) return 0;
		_Type ret = 0;
		for (auto i = array.begin(); i != array.end(); i++) {
			ret += (*i - mean_val) * (*i - mean_val);
		}
		return sqrt(ret / array.size() - 1);
	}

	template<typename _Type>
	void operate_linear(const std::vector<_Type>& input, std::vector<_Type>& output, _Type alpha, _Type beta) {
		output.resize(input.size());
		auto i = input.begin();
		auto j = output.begin();
		for (; i != input.end(); i++, j++) {
			*j = alpha * (*i) + beta;
		}
	}

};

class AS_MATH_API AsMathPoint {
public:
	AsMathPoint();
	AsMathPoint(double x, double y);

#ifdef OPENCV_DEP
	AsMathPoint(const cv::Point2i& p) : x(p.x), y(p.y) {}
	AsMathPoint(const cv::Point2f& p) : x(p.x), y(p.y) {}
	AsMathPoint(const cv::Point2d& p) : x(p.x), y(p.y) {}
#endif

public:
	double x;
	double y;

};

class AS_MATH_API AsMathPoint3D {
public:
	AsMathPoint3D();
	AsMathPoint3D(double x, double y, double z);

#ifdef OPENCV_DEP
	AsMathPoint3D(const cv::Point3i& p) : x(p.x), y(p.y), z(p.z) {}
	AsMathPoint3D(const cv::Point3f& p) : x(p.x), y(p.y), z(p.z) {}
	AsMathPoint3D(const cv::Point3d& p) : x(p.x), y(p.y), z(p.z) {}
#endif

public:
	double x;
	double y;
	double z;

public:
	AsMathPoint3D operator+(const AsMathPoint3D& p) const;
	AsMathPoint3D operator-(const AsMathPoint3D& p) const;

#ifdef OPENCV_DEP
	inline cv::Point3i toPoint3i() { return cv::Point3i(x, y, z); }
	inline cv::Point3f toPoint3f() { return cv::Point3f(x, y, z); }
	inline cv::Point3d toPoint3d() { return cv::Point3d(x, y, z); }
#endif

};

class AS_MATH_API AsMathVector3D {
public:
	AsMathVector3D(double x, double y, double z);
	AsMathVector3D(const AsMathPoint3D& p1, const AsMathPoint3D& p2);

public:
	double x;
	double y;
	double z;

public:
	double length() const; 

public:
	AsMathVector3D operator*(const AsMathVector3D& v) const;

};

class AS_MATH_API AsMathLine3D {
public:
	AsMathLine3D(const AsMathPoint3D& p0, const AsMathVector3D& v);

public:
	AsMathPoint3D p0;
	AsMathVector3D v;

private:
	double v_length;

public:
	double calcDistance(const AsMathPoint3D& p) const;

};

